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ABSTRACT 

We  summarize  some  recent  results  on  modeling  and  control 
of  ground  vehicles  navigating  in  high-speed  over  rough  ter¬ 
rain.  We  start  with  the  modeling  of  expert  race  (rally)  driv¬ 
ing  techniques,  and  we  then  propose  a  new  graph- search 
method  to  bridge  the  gap  between  the  path-planning  and 
trajectory  generation  layers  in  the  motion  planning  control 
hierarchy.  The  latter  result  is  of  independent  theoretical 
interest,  as  it  can  be  applied  to  any  graph  search  problem 
when  transitions  between  the  nodes  of  the  graph  depend  on 
the  prior  history  of  the  path. 


1.  INTRODUCTION 

Reducing  the  risk  for  human  lives  while  operating  in  a  haz¬ 
ardous  or  hostile  environment  has  led  to  the  development 
of  unmanned,  autonomous  and  semi-autonomous  vehicles 
for  both  commercial  and  military  applications.  A  typical 
military  mission  involves  driving  the  vehicle  from  point  A 
to  point  B,  avoiding  any  obstacles,  while  minimizing  the 
exposure  to  danger;  see  Fig.  1.  In  general,  this  involves 
driving  with  maximum  velocity. 


Fig.  1:  A  typical  military  mission  involves  an  autonomous  ve¬ 
hicle  entering  a  hazardous  area,  while  avoiding  obstacles  and/or 
minimizing  its  exposure  to  enemy  threats  and  countermeasures. 

Achieving  autonomous  operation  in  an  open,  unknown, 
unstructured  terrain,  while  minimizing  time  still  remains  a 
major  challenge  in  the  development  of  land  vehicles.  This 
is  due  to  the  uncertainty  of  the  environment  the  vehicle  op¬ 
erates  in,  the  poor  characterization  of  the  complex  physics 


at  the  interface  between  the  wheels  and  the  ground,  the  dy¬ 
namics  of  the  vehicle  itself,  and  the  overall  integration  of 
the  vehicle’s  control  algorithm  with  the  information  gath¬ 
ered  from  the  onboard  sensors.  Autonomous  driving  of 
wheeled  vehicles  at  high  speed,  in  particular,  adds  a  new 
level  of  complexity  due  to  the  time  constraints  imposed  by 
the  small  reaction  times  and  the  nonlinear  characteristics  of 
the  vehicle  at  those  extreme  regimes. 

The  emphasis  on  high-speed  is  motivated  by  the  advan¬ 
tages  in  terms  of  reaction  time,  minimization  of  exposure  to 
danger  for  vehicles  operating  in  enemy  territory,  increase  of 
supply  line  capacity,  reduction  in  delivery  time  of  materiel, 
etc  (Fig.  2(a)).  We  should  also  mention  the  fact,  confirmed 
by  several  Army  studies,  that  the  difficulty  of  successfully 
engaging  and  hitting  a  target  increases  disproportionately 
with  the  target  speed  (Fig.  2(b)).  In  summary,  increasing 
the  speed  of  a  vehicle  has  a  an  immediate  and  positive  cor¬ 
relation  on  the  success  of  its  mission. 


(a)  Convoys  throughout  can  be  in-  (b)  Success  of  hitting  a  target  de¬ 
creased  with  vehicle  speed.  creases  with  target  speed. 

Fig.  2:  A  safe  increase  of  the  vehicle’s  operating  speed  will  lead 
to  increase  convoy  speed  and  an  decrease  in  successful  ambush 
attacks. 

In  this  paper  we  summarize  several  recent  results  we 
have  developed  over  the  past  couple  of  years  on  the  path 
planning  and  motion  planning  problem  for  ground  vehi¬ 
cles  navigating  in  an  environment  full  of  obstacles  at  high 
speed.  At  the  core  of  our  approach  is  the  modeling  of  ex¬ 
pert  human  race  driving  techniques.  Indeed,  the  fastest 
off-road  vehicles  can  be  found  in  open  country  rally-cross 
racing.  Rally  racing  requires  a  great  deal  of  practice  and 
skill  (Frere  1992;  Jenkinson  1959).  Rally  race  drivers  op¬ 
erate  the  vehicle  at  the  limits  of  its  handling  capacity  with 
very  slim  safety  margins.  They  induce  oversteer  and  skid¬ 
ding  through  corners  to  optimize  their  trajectory  and  pre¬ 
pare  the  vehicle  for  the  next  maneuver;  see  Fig.  3. 
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Fig.  3:  Race  drivers  often  deliberately  induce  skidding  in  corners 
to  minimize  reaction  time  and  reorient  the  vehicle  correctly  for 
the  next  maneuver. 

At  the  level  of  the  vehicle  dynamics,  when  a  vehicle  op¬ 
erates  at  high  speed  and  over  rough  terrain,  the  common 
non- slipping  conditions  are  no  longer  valid.  We  have  used 
saturated  friction  models,  which  take  into  consideration  the 
possibility  of  the  wheel’s  reduced  or  complete  loss  of  adhe¬ 
sion  with  the  ground  (Canudas  de  Wit  et  al.  2003;  Tsiotras 
et  al.  2004;  Velenis  et  al.  2005).  Furthermore,  high-speed 
driving,  and  rally  racing  in  particular,  incorporates  several 
specialized  techniques  (e.g.,  power  oversteer,  Kansei  drift, 
braking  and  feint  drift,  pendulum  turn,  clutch  kick,  hand¬ 
brake  drift,  etc.)  which  are  non-common  during  everyday 
driving.  The  study  of  these  techniques,  their  understand¬ 
ing,  and  their  consistent  reproduction  is  also  of  prime  im¬ 
portance  in  this  context. 

At  the  path  and  motion  planning  levels,  several  of 
the  currently  available  path-planning  algorithms  for  au¬ 
tonomous  vehicles  (cf.  (LaValle  and  Kuffner  2001),  (Hsu 
et  al.  2002),  (Frazzoli  et  al.  2002))  are  tailored  towards  the 
high-end  of  the  unmanned  vehicle  spectrum.  Smaller-scale, 
low-cost  vehicles  may  not  have  the  on-board  resources  to 
implement  some  of  the  sophisticated  algorithms  available 
in  the  literature.  Intelligent  path  planners  for  these  vehi¬ 
cles  need  to  explicitly  address  the  constraints  imposed  by 
the  hardware  in  a  consistent  manner  and  as  early  on  in  the 
control  design  process  as  possible.  In  Section  3  we  propose 
a  computationally  efficient  method  to  incorporate  kinody- 
namic  constraints,  imposed  on  the  motion-planning  prob¬ 
lem  by  the  vehicle  dynamics  and  the  environment.  Along 
with  the  multi-resolution  path  planner  proposed  in  (Tsio¬ 
tras  and  Bakolas  2007;  Cowlagi  and  Tsiotras  2008,  2007; 
Bakolas  and  Tsiotras  2008),  these  results  constitute  a  com¬ 
putationally  efficient  overall  architecture  for  path  planning 
and  motion  planning  for  ground  vehicles  operating  in  high 
speed. 


2.  MODELING  EXPERT  RALLY  DRIVING 
TECHNIQUES 

In  this  section  by  presenting  empirical  information  on 
Trail-Braking,  one  of  the  commonly  used  rally  racing  ma¬ 
neuvers.  Trail-Braking  is  a  technique  used  by  rally  drivers 
to  negotiate  single  corners  at  high  speeds  (O’Neil  2006a). 
Typically,  an  average/novice  driver  negotiates  a  corner  by 
first  braking  to  regulate  the  speed,  then  by  releasing  the 


brakes  and  steering  the  vehicle  along  the  corner,  and  finally 
by  accelerating  after  the  exit  of  the  corner.  In  Trail-Braking 
deceleration  of  the  vehicle  by  braking  continues  even  af¬ 
ter  steering  has  commenced.  It  is  used  when  the  approach 
speed  to  the  corner  is  high.  An  example  of  the  steering 
and  throttle/braking  trail-braking  commands  to  negotiate  a 
90  deg  left  corner  as  shown  in  Fig.  4. 


Fig.  4:  Empirical  description  of  the  Trail-Braking  maneuver; 
from  (O’Neil  2006b). 


2.1  Minimum-Time  Cornering 

We  have  shown  (Velenis  et  al.  2007b, a,  2008)  that  a  TB 
maneuver  can  the  reproduced  as  the  result  of  a  minimum¬ 
time  cornering  problem  subject  to  certain  endpoint  bound¬ 
ary  conditions.  The  vehicle  model  used  is  a  standard 
single-track  vehicle  model  that  includes  load  transfer  ef¬ 
fects.  Load  transfer  (owing  to  inertia  effects  during  acceler¬ 
ation  and  deceleration)  are  crucial  when  maneuvering  over 
a  surface  with  reduced  friction,  and  cannot  be  neglected. 
The  model  is  summarized  in  the  equations  below 
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where  the  tire  forces  are  computed  from  fij  =  fizHij  (i  = 
j  =  x,y),  where  fiz  is  the  normal  load  at  each  of  the 
front  and  rear  axles,  and  /lij  is  the  longitudinal  and  lateral 
friction  coefficients  of  the  front  and  rear  tires.  The  friction 
coefficients  fiij  can  be  calculated  using,  for  instance,  Pace- 
jka’s  Magic  formula  (Bakker  et  al.  1987),  normalized  by 
the  corresponding  axle  normal  load  flz. 

The  front  and  rear  axle  normal  loads,  including  the  load 


transfer  effect,  are  given  by 


^  _  _ iRmg  -  hmgfiRx _  ^ 

L-\-h  cos  §  f-^Fy  sin  <5  j^Rx) 
fRz  =  mg-fFz,  (7) 

where  L  =  £f  +  £r  and  h  is  the  vertical  distance  of  the  center 
of  mass  of  the  vehicle  from  the  ground. 

By  applying  optimal  control  theory  and  imposing  that 
the  vehicle  completes  all  turning  before  exiting  the  corner, 
we  obtain  the  trajectory  shown  in  Fig.  5.  The  vehicle  in¬ 
duces  large  slip  angles  in  order  to  initiate  turning  about  its 
yaw  axis  early  on,  and  complete  all  turning  while  still  in¬ 
side  the  corner  (Fig.  6(b)).  The  strategy  is  quite  different 
than  neutral  steering  that  tends  to  keep  the  vehicle  slip  an¬ 
gle  small  (dash-dot  line  in  Fig.  6(b)). 
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Fig.  5:  Trail-Braking  cornering:  vehicle  trajectory  from  numeri¬ 
cal  optimization. 


Fig.  6:  Velocity  and  sideslip  angle  histories  for  a  Trail-Braking 
maneuver;  results  from  numerical  optimization. 

The  results  from  the  optimization  have  allowed  us  to  in¬ 
troduce  a  simple  parameterization  of  the  control  inputs  that 
simplifies  considerably  the  problem.  The  parameterization 
uses  specific  time  instances  to  switch  between  constant  or 
linear  functions  for  the  steering  and  throttle/braking  com¬ 
mands  as  shown  in  Fig.  7.  These  input  parameterization 
criteria  are  valid  for  a  wide  range  of  corner  geometries;  see 
Fig.  8. 

The  previous  results  have  been  validated  against  a  high- 
fidelity  vehicle  model  (CarSim)  including  all  four  wheels, 


(a)  (b) 


Fig.  7:  Parameterized  steering,  braking  and  throttle  inputs  for 
Trail-Braking. 

suspension  dynamics  etc.  The  optimization  criterion  used 
during  this  step  is  given  by 

^  Wt  tf  +  Wr  er  +  Wd  ed  (tf)  +  Wy/  ew  ( tf ) 

+  Wvev(tf)+Wyey(tf), 

where,  tf  is  the  final  time,  er  =  Ylk=\  er{h)  is  the  cumulative 
absolute  value  of  the  position  error  from  the  road  limits, 
ed(tf)  is  the  absolute  value  of  the  lateral  deviation  of  the  ve¬ 
hicle  from  the  inner  limit  of  the  road  at  tf,  ew(tf)  is  the  final 
absolute  orientation  error,  ev(tf)  is  the  final  absolute  lateral 
velocity  of  the  vehicle,  and  ey(tf)  is  the  final  absolute  yaw 
rate  error  of  the  vehicle.  The  weights  W*  (/  =  £,c,d,  i//,v,y) 
are  used  for  non-dimensionalization  and  to  adjust  the  rel¬ 
ative  significance  between  the  terms  in  the  right-hand- side 
of  (8).  The  results  of  these  high-fidelity  simulations,  and 
for  different  corner  geometries,  are  shown  in  Fig.  9. 


3.  KINODYNAMIC  MOTION  PLANNING 

The  exact  solution  of  the  path-planning  problem  in  an  un¬ 
structured,  dynamically  changing  environment  is  known  to 
be  computationally  intractable  (in  the  language  of  algorith¬ 
mic  complexity  it  is  “NP-hard”).  In  other  words,  the  on¬ 
line  computation  of  optimal,  safe,  collision-free  paths  (and 
the  corresponding  trajectories)  constitutes  an  insurmount¬ 
able  undertaking  with  current  computer  technology. 

To  tackle  the  computational  complexity,  in  the  literature 
the  problem  is  typically  decomposed  into  two  sequential 
steps,  namely,  the  path-generation  step,  followed  by  the 
trajectory-generation  step.  During  the  first  step  a  path  from 
a  given  initial  point  to  a  given  destination  in  the  environ¬ 
ment  is  computed,  such  that  the  path  does  not  intersect  any 
obstacles.  In  the  second  step,  a  time  parameterization  along 
the  path  yields  a  trajectory  that  must  be  followed  by  the 
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Fig.  8:  Optimal  trajectory  through  60,  90,  135,  and  180 deg  cor¬ 
ners;  results  from  numerical  optimization. 

vehicle.  During  the  last  step,  the  trajectory  needs  to  obey 
the  constraints  imposed  by  the  associated  vehicle  dynam¬ 
ics,  while  also  minimizing  a  certain  cost  function,  e.g.  time 
of  travel.  The  computation  of  a  suitable  trajectory  from  the 
starting  point  to  the  goal  destination  is  called  the  motion 
planning  problem. 

The  previous  decomposition  of  the  motion  planning 
problem  to  a  path  planning  problem  followed  by  a  suitable 
time  parameterization  is  somewhat  artificial.  Nonetheless, 
the  approach  is  popular  in  practice  since  its  use  simplifies 
the  overall  problem.  Apart  from  the  obvious  lack  of  op¬ 
timality,  this  approach  of  solving  separately  the  geometric 
and  dynamic  parts  of  the  problem,  may  also  lead  to  dynam¬ 
ically  infeasible  paths.  The  reason  being  that  the  geometric 
path  planner  has  no  prior  knowledge  of  the  dynamic  limi¬ 
tations  of  the  vehicle.  If  we  want  to  ensure  that  the  over¬ 
all  scheme  will  always  generate  feasible  paths,  we  have  to 
bridge  the  gap  between  the  geometric  and  dynamic  layers. 
This  can  only  be  achieved  if  certain  information  about  the 
dynamic  envelope  of  the  vehicle  is  passed  on  to  the  path 
planner  (dashed-line  in  Fig.  10). 

In  this  section  we  describe  a  new  scheme  to  include 
information  about  the  class  of  dynamically  feasible  paths 
early  on  (viz  at  the  geometric  layer).  Furthermore,  we  do 
this  in  an  numerically  efficient  manner  that  is  based  on  a 
non-trivial  modification  of  Dijkstra’s  algorithm  for  the  so¬ 
lution  of  shortest-path  problems  on  graphs.  In  that  sense, 
our  algorithm  is  of  more  general  interest  than  just  vehicle 
path/motion  planning.  It  can  be  used  to  search  for  shortest 


Fig.  9:  Trail-Braking  through  the  60,  90,  135,  and  180  deg  cor¬ 
ners;  results  of  high-fidelity  model  validation. 
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Fig.  10:  The  traditional  control  hierarchy  for  solving  robot  navi¬ 
gation  problems  (solid  arrows).  The  second  layer  is  typically  the 
most  difficult  one  as  it  needs  to  consider  the  full  nonlinear  dy¬ 
namics  of  the  vehicle.  Better  performance  can  be  achieved  by 
coupling  the  path-planning  and  motion-planning  (feasible  trajec¬ 
tory  generation)  layers,  perhaps  via  the  vehicle’s  modalities. 

paths  on  a  graph  whenever  the  node  transitions  depend  on 
the  prior  history  of  visited  nodes. 


3.1  Capturing  Curvature  Information 

Current  path  planning  algorithms  based  on  cell  decomposi¬ 
tions  of  the  obstacle-free  space  work  exceedingly  well  for 
generating  paths  when  no  kinematic  or  dynamic  constraints 
are  present.  A  survey  of  path  planning  algorithms  can  be 
found  in  (Latombe  1991;  LaValle  2006).  However,  the  mo¬ 
tion  of  the  actual  vehicle  must  obey  such  constraints.  For 
instance,  a  vehicle  always  has  a  certain  minimum  turn  ra¬ 
dius  rmin  (Fig.  1 1).  A  path  whose  curvature  does  not  satisfy 
this  curvature  constraint  cannot  be  followed  by  the  vehicle 
even  at  very  low  speeds.  Driving  a  high-speed  adds  addi¬ 
tional  restrictions  on  the  allowable,  feasible  paths.  Without 
additional  assumptions,  there  is  no  guarantee  that  a  feasible 
trajectory  satisfying  these  constraints  will  even  exist  within 
the  channel  of  cells  computed  by  a  (purely  geometric)  path 
planner. 

At  first  glance,  one  may  argue  that  this  is  only  an  arti¬ 
fact  of  an  inappropriate  choice  of  the  edge  cost  function 
in  the  associated  graph  representing  the  environment  and 
the  obstacles.  Below  we  provide  a  counter-example  to  this 


Fig.  11:  A  vehicle  has  a  minimum  turn  radius.  A  path  plan¬ 
ning  algorithm  needs  to  take  this  information  into  consideration. 
Most  path  planning  algorithms  do  not  do  that,  leading  to  infea¬ 
sible  paths  and,  subsequently,  to  poor  trajectory  tracking  perfor¬ 
mance. 


Fig.  12:  Counterexample  for  path  planning  without  kinematic 
constraint. 

argument. 

Consider  the  path  planning  problem  depicted  in  Fig.  12, 
where  S  denotes  the  initial  position,  G  denotes  the  goal,  and 
the  dark  areas  are  obstacles.  Consider  two  vehicles  A  and  B , 
whose  minimum  radii  of  turn  are  kinematically  constrained 
by  r£in  and  r®  in  respectively,  such  that  r£in  <  i  and  r^p  > 
L  Clearly,  the  dashed  path  in  Fig.  12  is  feasible  for  vehicle 
A,  but  not  for  vehicle  B.  A  path  planning  algorithm  for  B 
ought  to  result  in  the  bold  path  shown  in  Fig.  12. 

Figure  13  depicts  the  same  problem  with  a  uniform  cell 
decomposition.  The  channel  containing  the  dashed  path  of 
Fig.  12  is  denoted  by  cells  with  bold  outlines.  Such  a  chan¬ 
nel  is  obviously  not  traversable  by  vehicle  B.  However, 
notice  that  no  pair  of  successive  cells  is  by  itself  infeasible, 
i.e.,  a  channel  defined  by  two  successive  cells  alone  always 
contains  a  feasible  path.  Stated  differently,  for  any  two  ad¬ 
jacent  cells,  there  is  no  cell-dependent  property  associated 
with  the  two  adjacent  cells  that  can  be  penalized  by  an  edge 
cost  function  in  order  to  prevent  the  graph  search  from  gen¬ 
erating  a  channel  such  as  the  one  shown  in  Fig.  13(a). 

It  may  be  further  argued  that  a  feasible  path  is  guaran¬ 
teed  to  exist  in  any  channel  if  the  dimensions  of  the  cells 
are  large  enough.  Indeed,  in  (Bereg  and  Kirkpatrick  2005) 
it  is  shown  that  a  curvature-bounded  path  with  local  cur¬ 


(a)  (b) 


Fig.  13:  (a)  No  pair  of  successive  cells  is  by  itself  infeasible;  (b) 
Cells  are  too  large,  all  cells  are  MIXED. 

vature  less  than  or  equal  to  1  / n  exists  in  a  polygonal 
channel  if  the  width  w  of  the  channel  satisfies  the  inequal¬ 
ity  w  >  Trmin,  where  T  satisfies  a  certain  polynomial  equa¬ 
tion.  The  above  counter-example  also  serves  to  illustrate 
that  such  a  choice  of  cells  may  be  too  restrictive  in  prac¬ 
tice.  Figure  13(b)  illustrates  that  the  dimensions  of  the  cells 
may  be  too  large  to  capture  details  of  the  environment,  i.e., 
the  number  of  mixed  cells  could  be  too  large  for  the  cell 
decomposition  to  be  useful  for  path  planning. 


3.2  Working  with  Multiple  Cells 

In  this  paper,  we  propose  the  following  approach  to  plan 
a  path  using  cell  decompositions,  while  incorporating  path 
curvature  information:  On  the  topological  graph  associated 
with  a  given  cell  decomposition,  we  define  a  cost  function 
based  on  tuples  (i.e.,  histories)  of  nodes,  for  some  fixed 
k  >  2,  such  that  the  elements  of  each  k— tuple  are  pairwise 
adjacent.  The  question  of  feasibility  of  traversal  through 
tuples  of  cells  (rather  than  traversal  through  two  suc¬ 
cessive  cells  only)  allows  for  more  general  definitions  of 
“edge”  cost  functions.  In  particular,  we  can  introduce  suit¬ 
able  costs  that  capture  the  maximum  approximate  curvature 
for  any  path  lying  inside  the  channel.  As  a  result,  we  can 
ensure  that  a  feasible  trajectory  will  always  exist  inside  the 
computed  channel  of  cells  at  the  geometric,  path-planning 
layer,  even  before  invoking  the  motion-planning  task. 


3.3  Algorithm  For  History-Based  Search 

Next,  we  briefly  describe  an  algorithm  for  searching  for 
optimal  paths  in  graphs  with  a  history -based  cost  function. 
A  rigorous  description,  along  with  analytical  results  on  the 
optimality  and  performance  of  the  algorithm,  is  given  in 
(Cowlagi  and  Tsiotras  2009). 

Consider  a  uniform  cell  decomposition  %  consisting  of 
N  cells.  We  may  construct  a  graph  =  (^,<f),  such  that 
each  element  in  the  set  of  nodes  f  corresponds  to  a  unique, 
obstacle-free  cell.  We  label  the  nodes  as  1,2, ...  ,N.  Two 
nodes  are  adjacent  if  the  corresponding  cells  are  geometri¬ 
cally  adjacent1.  The  edge  set  S  consists  of  all  pairs  (ij), 
ij  £  y  with  nodes  i  and  j  adjacent.  We  now  describe  two 
path  planning  problems  on  graph  associated  with 

Let  g(-)  be  an  edge  cost  function  defined  for  each  pair 

1  We  consider  4-connectivity  for  this  work,  that  is,  cells  that  have  two 
vertices  in  common  are  said  to  be  adjacent. 


of  adjacent  nodes  in  £f.  For  a  given  H  >  1,  let  gu+ 1(-)  be 
cost  function  with  each  sequence  of  nodes  of  length  H  +  2, 
in  which  pairs  of  successive  nodes  are  adjacent.  For  given 
initial  and  terminal  nodes  is,  z’g  £  an  admissible  path 
n=  C/’o  ^  7? ^  •  •  •  Jp)  in  Sf  is  a  sequence  of  nodes  in  which 
all  pairs  of  successive  nodes  are  adjacent. 

Problem  1.  Let  the  cost  of  an  admissible  path  n  be 

m  =  £g(ut  i,;?)).  (9) 

k=i 

Find  an  admissible  path  7T*  in  such  that  /(tt*)  <  /(7r)  for 
every  admissible  path  n  in  Sf . 

Problem  2.  Let  the  cost  associated  with  n  be 


p 

</(n)  =  X  gH+\((jk-H-\,jk-H,---Jk))-  (10) 

k=H+ 1 

Find  an  admissible  path  7 r*  in  Sf  such  that  {n*  )  <  /  w 
for  every  admissible  path  7T  in  . 

Problem  1  is  the  usual  graph  search  problem  associated 
with  a  cell  decomposition.  A  class  of  algorithms  used  to 
solve  Problem  1  are  the  label  correcting  algorithms.  A  la¬ 
bel  correcting  algorithm  progressively  searches  for  the  least 
cost  path  starting  from  is  and  ending  at  node  i  G  Y,  by  it¬ 
eratively  reducing  an  estimate  d{i)  of  the  least  cost  to  i, 
called  the  label  of  the  node  i.  The  algorithm  also  maintains 
a  list  of  nodes,  called  the  OPEN  list,  whose  labels  can  po¬ 
tentially  be  reduced  from  their  current  value,  as  well  as  a 
backpointer  b(i ),  which  records  the  immediate  predecessor 
of  each  node  i  G  'f'  in  the  optimal  path  from  is  to  i.  De¬ 
tailed  descriptions  of  the  general  label  correcting  algorithm 
and  particular  examples,  such  as  the  Bellman-Ford,  Dijk- 
stra,  and  A*  algorithms,  may  be  found  in  (Bertsekas  2000), 
(Cormen  et  al.  2001)  and  (LaValle  2006). 

On  the  other  hand,  Problem  2  encapsulates  the  history- 
based  search  that  we  would  like  to  perform.  The  simplest 
example  of  gH+i  is  a  binary  cost  function,  i.e.,  one  which 
classifies  aH  +  2-history  of  cells  as  “good”  (low  value)  or 
“bad”  (high  value).  Figure  14  illustrates  histories  (unique 
up  to  translation,  rotation,  and  reflection  operations)  that 
may  be  classified  as  “good”  for  H  =  3  . 

The  proposed  algorithm  to  solve  Problem  2  is  an  exten¬ 
sion  of  the  standard  label  correcting  algorithm  described 
above.  While  a  formal  description  of  the  proposed  algo¬ 
rithm  may  be  found  in  (Cowlagi  and  Tsiotras  2009),  the  pri¬ 
mary  idea  is  that  of  recording  multiple  histories  (of  length 
H  - hi)  of  each  node  ,  and,  consequently,  multiple  labels. 
The  multiple  histories  in  the  proposed  algorithm  replace 
the  backpointer  of  the  standard  label  correcting  algorithm. 
Figure  15  illustrates  this  idea. 

Suppose  that  the  node  being  processed  at  a  particular  in¬ 
stant  during  the  execution  of  the  algorithm  corresponds  to 
the  cell  with  the  bold,  dark  outline  in  Fig.  15(a).  Suppose 


Fig.  14:  Illustration  of  tiles  of  “good”  histories  for  H  =  3 


Fig.  15:  Idea  of  recording  multiple  histories. 

that  the  cost  function  under  consideration  is  the  binary  cost 
function  introduced  above,  with  H  =  3,  such  that  the  histo¬ 
ries  shown  in  Fig.  14  have  low  cost.  Next,  assume  that  the 
cost  of  the  channel  containing  the  bold  path  in  Fig.  15(a) 
has  a  slightly  lower  cost  than  that  of  the  channel  containing 
the  dotted  path.  Figure  15(b)  shows  the  next  iteration  of 
the  algorithm,  where  the  new  bold  cell  corresponds  to  the 
node  being  processed.  Clearly,  the  channel  containing  the 
bold  path  has  a  high  cost,  while  the  channel  containing  the 
dotted  path  has  a  low  cost.  However,  if  the  algorithm  had 
discarded  the  channel  with  the  dotted  path  in  the  earlier  iter¬ 
ation,  then  in  the  next  iteration,  it  would  have  been  “stuck” 
with  the  channel  containing  the  bold  path,  thus  incurring 
a  high  cost  despite  a  low  cost  alternative  being  available. 
To  avoid  such  situations,  the  proposed  algorithm  records 
all  histories  of  a  given,  finite  length,  and  maintains  labels 
for  each  of  those  histories.  When  a  particular  node  is  pro¬ 
cessed,  its  label  is  computed  using  all  histories  and  labels 
of  its  predecessors,  thus  ensuring  that  no  history  is  “forgot¬ 
ten”. 

Figure  16  shows  the  application  of  the  proposed  algo¬ 
rithm  to  the  motivating  problem  of  Fig.  12,  using  the  bi¬ 
nary  cost  function  discussed  before.  The  channel  of  cells 
marked  in  red  is  the  result  of  the  proposed  algorithm,  while 
the  channel  marked  in  blue  is  the  result  of  performing  a 
standard  graph  search  on  using  a  cost  defined  on  the 
edge  set  only  (H  =  0) .  The  corresponding  geometric  paths 


of  minimum  possible  curvature  inside  the  channels  are  also 
shown  in  the  same  figure.  The  red  curve  is  longer  of  course, 
but  it  has  lower  maximum  curvature. 


Fig.  16:  The  example  of  Fig.  12  solved  using  the  proposed  algo¬ 
rithm. 

Figure  17  shows  a  more  interesting  example,  where  the 
environment  is  cluttered  with  randomly  distributed  obsta¬ 
cles.  As  before,  the  channel  consisting  of  red  cells  is  the 
result  of  the  proposed  algorithm,  whereas  the  channel  con¬ 
sisting  of  cells  marked  in  blue  is  the  result  of  perform¬ 
ing  a  standard  graph  search.  The  corresponding  geomet¬ 
ric  paths  of  minimum  possible  curvature  inside  the  chan¬ 
nels  are  also  shown  in  the  same  figure.  Most  interestingly, 
Fig.  17(b)  shows  the  corresponding  optimal  velocity  profile 
from  the  solution  of  the  minimum-time  problem  along  each 
path  (Velenis  and  Tsiotras  2008).  Although  the  red  path  is 
longer,  a  vehicle  following  this  path  will  take  less  time  than 
a  vehicle  following  the  shorter  (but  with  more  and  sharper 
turns)  blue  path.  This  example  demonstrates  the  benefits 
of  the  proposed  algorithm  for  the  solution  motion  planning 
problems. 


(b)  Optimal  velocity  profile 

Fig.  17:  Example  of  a  cluttered  environment. 

In  closing,  it  is  worth-mentioning  that  it  is  easy  to  extend 
the  proposed  algorithm  to  search  for  an  optimal  channel  as 
well  as  for  a  kinodynamically  feasible  path  lying  within 
that  channel  at  the  same  time.  Figure  18  shows  simulation 


results  of  the  application  of  such  an  extension  to  the  gener¬ 
ation  of  paths  with  maximum  curvature  l/rmjn,  for  a  given 
rmin-  Note  that,  unlike  the  results  in  Fig.  16,  the  search  for 
the  optimal  channel  and  construction  of  a  geometric  path 
within  that  channel  was  done  simultaneously  in  the  case  of 
Fig.  18. 


Fig.  18:  Resultant  channels  and  trajectories  for  different  curva¬ 
ture  bounds.  The  cell  marked  in  blue  is  the  start  cell.  The  envi¬ 
ronment  is  similar  to  that  in  the  motivating  example  of  Fig.  12. 
Channels  and  trajectories  computed  simultaneously. 


CONCLUSIONS 


We  present  recent  results  on  the  problem  of  path  and  tra¬ 
jectory  generation  for  high-speed  vehicles.  In  the  first  part 
of  the  paper  we  present  a  mathematical  formalism  that  uses 
optimal  control  theory  to  generate  trail-braking,  high-speed 
maneuvers  for  a  large  variety  of  corner  geometries.  In  the 
second  part  of  the  paper  we  propose  a  new  algorithm  for 
path  planning  that  takes  into  account  the  kinodynamic  con¬ 
straints  imposed  by  the  vehicle  dynamics  and  or  the  en¬ 
vironment.  The  algorithm  can  be  used  in  cases  when  the 
computational  resources  on-board  the  vehicle  are  limited. 
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